import js.tinyvm.SpecialConstants;
import lejos.nxt.*;
import lejos.nxt.addon.SensorSelector;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.RegulatedMotorListener;
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.MoveListener;
import lejos.robotics.navigation.MoveProvider;
      
public class Test {
	
	private static NXTRegulatedMotor rightWheel = Motor.A;
	private static NXTRegulatedMotor leftWheel = Motor.B;
//	private static NXTRegulatedMotor mover = Motor.C;
//	private static LightSensor lightS = new LightSensor(SensorPort.S1);
//	private static TouchSensor touchS = new TouchSensor(SensorPort.S2);
	private static UltrasonicSensor sonicS = new UltrasonicSensor(SensorPort.S3);
//	private static SoundSensor soundS = new SoundSensor(SensorPort.S4);
	
	private static final int TRACKING_LINE_VALUE= 45;
	
	public static void main (String[] args)
			throws InterruptedException {
		
		rightWheel.setSpeed(2000000000);
		leftWheel.setSpeed(2000000000);

//		rightWheel.forward();
//		leftWheel.forward();
		

		rightWheel.addListener(new RegulatedMotorListener() {
			
			@Override
			public void rotationStopped(RegulatedMotor arg0, int arg1, boolean arg2,
					long arg3) {
				LCD.drawString("Rotation Stopped ",0,3);
				LCD.drawString(arg0.getSpeed()+" "+arg1+" "+arg2, 0,4);
			}
			
			@Override
			public void rotationStarted(RegulatedMotor arg0, int arg1, boolean arg2,
					long arg3) {
				LCD.drawString("Rotation stopped ",0,5);
				LCD.drawString(arg0.getSpeed()+" "+arg1+" "+arg2, 0,6);
				
			}
		});
		leftWheel.addListener(new RegulatedMotorListener() {
			
			@Override
			public void rotationStopped(RegulatedMotor arg0, int arg1, boolean arg2,
					long arg3) {
				LCD.drawString("Rotation Stopped ",0,7);
				LCD.drawString(arg0.getSpeed()+" "+arg1+" "+arg2, 0,8);
			}
			
			@Override
			public void rotationStarted(RegulatedMotor arg0, int arg1, boolean arg2,
					long arg3) {
				LCD.drawString("Rotation stopped ",0,9);
				LCD.drawString(arg0.getSpeed()+" "+arg1+" "+arg2, 0,10);
				
			}
		});
		while (!Button.ESCAPE.isPressed())
		{
//			LCD.drawInt(lightS.getLightValue(), 0, 0);
//			LCD.drawInt(lightS.getNormalizedLightValue(), 0, 1);
//			LCD.drawString("Touch: " + touchS.isPressed(), 0, 2);
//			LCD.drawString("Distance:" + sonicS.getDistance()+ " "+sonicS.getRange(), 0, 3);
//			LCD.drawString("Distance:" + sonicS.getUnits(), 0, 4);
//			LCD.drawString("Distance:" + sonicS.getActualMode()+" "+sonicS.getMode(), 0, 5);
//			LCD.drawString("Sound: "+ soundS.readValue(),0,6);
			

//			Thread.sleep(200);
//			LCD.refresh();
			
			if (sonicS.getDistance() > 50)
			{
				leftWheel.setSpeed(2000000000);
				rightWheel.setAcceleration(500);
				leftWheel.setAcceleration(500);	
				rightWheel.forward();
				leftWheel.forward();
			}
			else
			{
				rightWheel.setAcceleration(6000);
				leftWheel.setAcceleration(6000);
				
				leftWheel.setSpeed(300);
				leftWheel.backward();
				rightWheel.rotate(45);
			}
		}
		//Button.waitForPress();
		return;
	}
	
	private static void checkRight45Degrees(){
//		rightWheel.rotate(angle)
	}
	
	private static void checkLeft45Degrees(){
		
	}
	
	
	private static void setMovingDefault() {
		rightWheel.setSpeed(900);
		rightWheel.setAcceleration(99);
		leftWheel.setSpeed(900);
		leftWheel.setAcceleration(100);
	}
 
	static void stopMoving () {
		rightWheel.stop();
		leftWheel.stop();
	}
	
	public static void rotateRight()
	{
		stopMoving();
//		leftWheel
		
	}
}